#include "svpwm.h"
#include "arm_math.h"


static void SvpwmSectorJudgment(PSvpwm_Struct pSvpwm);
static void GetVectorDuration(PSvpwm_Struct pSvpwm);
static void SvpwmGenerate(PSvpwm_Struct pSvpwm);


/**
 * @brief   SVPWM模块
 *
 * @param   pSvpwm		Svpwm结构体
 *
 * @return  none
 */
void SvpwmContorol(PSvpwm_Struct pSvpwm, float u_alpha, float u_beta)
{
    pSvpwm->u_alpha = u_alpha;
    pSvpwm->u_beta  = u_beta;
	
    // 1. 扇区判断
    SvpwmSectorJudgment(pSvpwm);
	
    // 2. 计算矢量作用时长
    GetVectorDuration(pSvpwm);
    
	// 3. SVPWM生成
    SvpwmGenerate(pSvpwm);
}

/**
 * @brief   根据需求向量判断 和向量 的扇区位置
 *
 * @param   pSvpwm		Svpwm结构体
 *
 * @return  none
 */
static void SvpwmSectorJudgment(PSvpwm_Struct pSvpwm)
{
    uint8_t a;
    uint8_t b;
    uint8_t c;
    uint8_t sector;

    pSvpwm->u1 =  pSvpwm->u_beta;
	pSvpwm->u2 =  pSvpwm->u_alpha * SVPWM_SQRT3_2 - pSvpwm->u_beta / 2.0f;
	pSvpwm->u3 = -pSvpwm->u_alpha * SVPWM_SQRT3_2 - pSvpwm->u_beta / 2.0f;


    if (pSvpwm->u1 > 0) 	a = 1;
	else					a = 0;

    if (pSvpwm->u2 > 0)		b = 1;
    else					b = 0;

    if (pSvpwm->u3 > 0)		c = 1;
    else					c = 0;

	sector = 4*c + 2*b + a;
	switch (sector)
	{
		case 3:
			pSvpwm->sector = 1;
			break;
		case 1:
			pSvpwm->sector = 2;
			break;
		case 5:
			pSvpwm->sector = 3;
			break;
		case 4:
			pSvpwm->sector = 4;
			break;
		case 6:
			pSvpwm->sector = 5;
			break;
		case 2:
			pSvpwm->sector = 6;
			break;
    }
}

/**
 * @brief   根据扇区位置计算相邻向量的作用时间
 *
 * @param   pSvpwm		Svpwm结构体
 *
 * @return  none
 */
static void GetVectorDuration(PSvpwm_Struct pSvpwm)
{
    switch (pSvpwm->sector) {
        case 1:
            pSvpwm->t4 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u2;
            pSvpwm->t6 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u1;
            pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t4 - pSvpwm->t6) / 2.0f;
            break;
        case 2:
            pSvpwm->t2 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u2;
            pSvpwm->t6 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u3;
            pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t2 - pSvpwm->t6) / 2.0f;
            break;
        case 3:
            pSvpwm->t2 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u1;
            pSvpwm->t3 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u3;
            pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t2 - pSvpwm->t3) / 2.0f;
            break;
        case 4:
            pSvpwm->t1 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u1;
            pSvpwm->t3 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u2;
            pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t1 - pSvpwm->t3) / 2.0f;
            break;
        case 5:
            pSvpwm->t1 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u3;
            pSvpwm->t5 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u2;
            pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t1 - pSvpwm->t5) / 2.0f;
            break;
        case 6:
            pSvpwm->t4 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u3;
            pSvpwm->t5 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u1;
            pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t4 - pSvpwm->t5) / 2.0f;
            break;
        default:
            break;
    }
}

/**
 * @brief   根据相邻向量的作用时间 转换为 定时器PWM占空比
 *
 * @param   pSvpwm		Svpwm结构体
 *
 * @return  none
 */
static void SvpwmGenerate(PSvpwm_Struct pSvpwm)
{
    float ta;
    float tb;
    float tc;
    switch (pSvpwm->sector)
	{
        case 1:
            ta = pSvpwm->t4 + pSvpwm->t6 + pSvpwm->t7;
            tb = pSvpwm->t6 + pSvpwm->t7;
            tc = pSvpwm->t7;
            break;
        case 2:
            ta = pSvpwm->t6 + pSvpwm->t7;
            tb = pSvpwm->t2 + pSvpwm->t6 + pSvpwm->t7;
            tc = pSvpwm->t7;
            break;
        case 3:
            ta = pSvpwm->t7;
            tb = pSvpwm->t2 + pSvpwm->t3 + pSvpwm->t7;
            tc = pSvpwm->t3 + pSvpwm->t7;
            break;
        case 4:
            ta = pSvpwm->t7;
            tb = pSvpwm->t3 + pSvpwm->t7;
            tc = pSvpwm->t1 + pSvpwm->t3 + pSvpwm->t7;
            break;
        case 5:
            ta = pSvpwm->t5 + pSvpwm->t7;
            tb = pSvpwm->t7;
            tc = pSvpwm->t1 + pSvpwm->t5 + pSvpwm->t7;
            break;
        case 6:
            ta = pSvpwm->t4 + pSvpwm->t5 + pSvpwm->t7;
            tb = pSvpwm->t7;
            tc = pSvpwm->t5 + pSvpwm->t7;
            break;
    }
	
    pSvpwm->SetChannelAHighLeaveTime(ta);
    pSvpwm->SetChannelBHighLeaveTime(tb);
    pSvpwm->SetChannelCHighLeaveTime(tc);
	
}

/**
 * @brief   输出扇区位置
 *
 * @param   pSvpwm		Svpwm结构体
 *
 * @return  none
 */
uint8_t GetSVPWMSector(PSvpwm_Struct pSvpwm)
{
    return pSvpwm->sector;
}


